///////////////////////////////////////////////////////////////////////////////////////////////////
/// \file formic/numeric/matrix.cpp
///
/// \brief   implementation for matrix mathematics functions
///
///////////////////////////////////////////////////////////////////////////////////////////////////

#include<algorithm>
#include<complex>
#include<cstdlib>
#include<cmath>

#include<formic/numeric/numeric.h>
#include<formic/numeric/matrix.h>
#include<formic/numeric/mantissa_exponent.h>
#include<formic/lapack/interface.h>
#include<formic/exception.h>

///////////////////////////////////////////////////////////////////////////////////////////////////
/// \brief   Computes a matrix's inverse and determinant using an LU decomposition.
///
/// \param[in]     n                       the dimension of the matrix A
/// \param[out]    det                     on exit, the matrix's determinant
/// \param[in,out] A          size n*n     On entry, the matrix to be inverted.
///                                        On exit, the inverse.
/// \param[out]    work       size n*n     work space for computing the inverse
/// \param[out]    iwork      size 2*n     integer work space
///
///////////////////////////////////////////////////////////////////////////////////////////////////
template <class S> void formic::matrix_inverse_lu(const int n,
                                                  S & det,
                                                  S * const A,
                                                  S * const work,
                                                  int * const iwork) {

  // use some of the integer work space for pivoting during the LU decomposition
  int * const ipiv = iwork;

  // use some of the integer work space for inverting the LU pivoting
  int * const inv_perm = iwork + n;

  // copy the matrix into the workspace
  formic::xcopy(n*n, A, 1, work, 1);

  // compute the LU decomposition of the matrix
  int info;
  formic::xgetrf(n, n, work, n, ipiv, info);
  if (info < 0)
    throw formic::Exception("formic::xgetrf failed with error code %i in formic::matrix_inverse_lu") % info;
  if (info > 0)
    throw formic::Exception("U(%i,%i) is exactly zero in formic::matrix_inverse_lu, thus the matrix is not invertable") % info % info;

  // since FORTRAN counts from 1, subtract 1 from each of the pivot array values
  for (int i = 0; i < n; i++)
    ipiv[i] -= 1;

  // compute the determinant
  det = formic::unity(S());
  for (int i = 0; i < n; i++)
    if ( i != ipiv[i] )
      det *= - work[i*n+i];
    else
      det *=   work[i*n+i];
  //if (std::abs(det) < 1.0e-12)
  //  throw pcps::SingularMatrixException( (boost::format("matrix determinant is zero in pcps::matrix_inverse_lu, thus the matrix is not invertable")).str() );

  // determine the inverse of the pivot array's permutation
  for (int i = 0; i < n; i++)
    inv_perm[i] = i;
  for (int i = n-1; i >= 0; i--)
    std::swap(inv_perm[i], inv_perm[ipiv[i]]);

  // compute the inverse of the upper triangle matrix
  for (int i = 0; i < n; i++)
  for (int j = i; j < n; j++) {
    A[i*n+j] = ( i == j ? formic::unity(S()) : formic::zero(S()) );
    for (int k = i; k < j; k++)
      A[i*n+j] -= A[i*n+k] * work[j*n+k];  // work indices transposed due to FORTRAN
    A[i*n+j] /= work[j*n+j];
  }

  // compute the inverse of the lower triangle matrix (whose diagonal elements are one and are not stored)
  for (int i = 1; i < n; i++)
  for (int j = i-1; j >= 0; j--) {
    A[i*n+j] = -work[j*n+i];  // work indices transposed due to FORTRAN
    for (int k = j+1; k < i; k++)
      A[i*n+j] -= A[k*n+j] * work[k*n+i];  // work indices transposed due to FORTRAN
  }

  // compute the product of the inverse triangular matrices
  {
    const S * const U_inv = A;
    const S * const L_inv = A;
    for (int i = 0; i < n; i++)
    for (int j = 0; j < n; j++) {
      // term with diagonal of L_inv (which equals one)
      work[i*n+j] = ( i <= j ? U_inv[i*n+j] : formic::zero(S()) );
      // other terms
      for (int k = std::max(i, j+1); k < n; k++)
        work[i*n+j] += U_inv[i*n+k] * L_inv[k*n+j];
    }
  }

  // Multiply by the inverse of the permutation matrix (column reordering)
  // We also transpose the final matrix to account for FORTRAN indexing
  for (int i = 0; i < n; i++)
  for (int j = 0; j < n; j++)
    A[j*n+i] = work[i*n+inv_perm[j]];

}

// explicitly instantiate the function
template void formic::matrix_inverse_lu(const int n,
                                        double & det,
                                        double * const A,
                                        double * const work,
                                        int * const iwork);
template void formic::matrix_inverse_lu(const int n,
                                        std::complex<double> & det,
                                        std::complex<double> * const A,
                                        std::complex<double> * const work,
                                        int * const iwork);

///////////////////////////////////////////////////////////////////////////////////////////////////
/// \brief   computes the determinant of a matrix using LU decomposition
///
/// \param[in]       n       The matrix dimension.
/// \param[in,out]   A       On entry, the matrix whose determinant we want.  Overwritten on exit. Size n*n.
/// \param[out]      iwork   Work space for pivoting during the LU decomposition, size n.
///
/// \return the determinant in mantissa/exponent format
///
///////////////////////////////////////////////////////////////////////////////////////////////////
template <class S> formic::MantExp<S> formic::matrix_determinant_lu(const int n, S * const A, int * const iwork) {

  // compute the LU decomposition of the matrix
  int info;
  formic::xgetrf(n, n, A, n, iwork, info);
  if (info < 0)
    throw formic::Exception("formic::xgetrf failed with error code %i in formic::matrix_determinant_lu") % info;

  // since FORTRAN counts from 1, subtract 1 from each of the pivot array values
  for (int i = 0; i < n; i++)
    iwork[i] -= 1;

  // compute the determinant
  formic::MantExp<S> det(formic::unity(S()));
  for (int i = 0; i < n; i++)
    if ( i != iwork[i] )
      det *= - A[i*n+i];
    else
      det *=   A[i*n+i];

  // return the determinant value
  return det;

}

// explicitly instantiate the function
template formic::MantExp<double               > formic::matrix_determinant_lu(const int, double               * const, int * const);
template formic::MantExp<std::complex<double> > formic::matrix_determinant_lu(const int, std::complex<double> * const, int * const);


///////////////////////////////////////////////////////////////////////////////////////////////////
/// \brief   Computes the hermitian matrix A^(-1/2) for a hermitian, positive definite matrix A
///
/// \param[in]       n                     the dimension of the matrix A
/// \param[in,out]   A        size n*n     On entry, the matrix to be negative halved.
///                                        On exit, A^(-1/2).
///
///////////////////////////////////////////////////////////////////////////////////////////////////
template <class S> void formic::matrix_neg_half(const int n, S * const A) {

  int info;
  const int lwork = std::max(5,n) * n;
  std::vector<S> work(lwork, formic::zero(S()));
  std::vector<S> work2(lwork, formic::zero(S()));
  std::vector<double> rwork(3*n, 0.0);
  std::vector<double> eval(n, 0.0);

  // diagonalize the matrix, after which the eigenvectors will be in the rows the matrix
  formic::xsyev('V', 'U', n, A, n, &eval.at(0), &work.at(0), lwork, &rwork.at(0), info);
  if (info != 0)
    throw formic::Exception("formic::xsyev failed with error code %i in formic::matrix_neg_half") % info;

  // compute the A^(-1/2) matrix
  for (int i = 0; i < n; i++) {
    const double x = 1.0 / std::sqrt(eval[i]);
    for (int j = 0; j < n; j++) {
      work[i*n+j] = A[j*n+i];
      work2[i*n+j] = A[i*n+j] * x;
    }
  }
  formic::xgemm('N', 'N', n, n, n, formic::unity(S()), &work2.at(0), n, &work.at(0), n, formic::zero(S()), A, n);

}

template void formic::matrix_neg_half(const int n, double * const A);
template void formic::matrix_neg_half(const int n, std::complex<double> * const A);
